#include "nw_definitions.h"
//#include "nw_utility.h"

static NW_QUEUE *new_nw_queue(const unsigned char addr, NW_QUEUE **head) {
	NW_QUEUE *q = malloc(sizeof(NW_QUEUE));
	memset(q, 0, sizeof(NW_QUEUE));
	NW_QUEUE *tail = *head;

	q->addr = addr;
	q->next_seq = 1;
	q->pkts = vector_new();
	q->acks = vector_new();
	q->ack_timeout = (CnetTime)NW_ACK_TIMEOUT_DEF;

	while ((tail != NULL) && (tail->next != NULL))
		tail = tail->next;
	
	if (*head == NULL)
		*head = q;
	else
		tail->next = q;

	q->next = NULL;
	//CNET_start_timer(NW_SPEED_TIMER, (CnetTime)NW_SPEED_TIMEOUT, (CnetData)q->addr);
	//CNET_start_timer(NW_APP_TIMER, (CnetTime)NW_APP_FACTOR / NW_APP_DEF_ADJ, (CnetData)q->addr);
	return q;
}

static NW_QUEUE *get_queue_by_addr(const unsigned char addr, NW_QUEUE *head) {
	NW_QUEUE *q = head;
	
	while (q != NULL) {
		if (q->addr == addr)
			return q;
		q = q->next;
	}
	
	return NULL;
}

static void qpkt_append(VECTOR v, NW_QUEUED_PKT *qpkt) {
       vector_append(v, qpkt, sizeof(NW_QUEUED_PKT));
}

static void qpkt_insert(VECTOR v, NW_QUEUED_PKT *qpkt, const uint16_t pos) {
	NW_QUEUED_PKT *tmpqp;
	uint16_t i;
	size_t len;
	
	// append last queue item to queue
	len = sizeof(NW_QUEUED_PKT);
	tmpqp = (NW_QUEUED_PKT *)vector_peek(v, vector_nitems(v) - 1, &len);
	vector_append(v, tmpqp, sizeof(NW_QUEUED_PKT));
	
	// shift each queue item up until pos
	for (i = vector_nitems(v) - 2; i > pos; i--) {
		len = sizeof(NW_QUEUED_PKT);
		tmpqp = (NW_QUEUED_PKT *)vector_peek(v, i - 1, &len);
		vector_replace(v, i, tmpqp, sizeof(NW_QUEUED_PKT));
	}
	
	// replace pos with new item
	vector_replace(v, i, qpkt, sizeof(NW_QUEUED_PKT));
}

static NW_QUEUED_PKT *qpkt_remove(VECTOR v, const uint16_t pos) {
	size_t len = sizeof(NW_QUEUED_PKT);

	return vector_remove(v, pos, &len);
}

static NW_QUEUED_PKT *qpkt_pop(VECTOR v) {
	size_t len = sizeof(NW_QUEUED_PKT);

	return vector_remove(v, 0, &len);
}

static bool qpkt_empty(VECTOR v) {
	return (vector_nitems(v) == 0);
}

static bool qpkt_full(VECTOR v) {
	return (vector_nitems(v) >= NW_WINDOW_SIZE);
}

static int get_qpkt_pos_by_seq(VECTOR v, const uint32_t seq) {
	uint16_t pos;
	size_t len;

	for (pos = 0; pos < vector_nitems(v); pos++) {
		len = sizeof(NW_QUEUED_PKT);
		if (((NW_QUEUED_PKT *)vector_peek(v, pos, &len))->pkt.seq == seq)
			return pos;
	}

	return -1;
}

static int get_qpkt_insert_pos_by_seq(VECTOR v, const uint32_t seq) {
	uint16_t pos;
	size_t len;

	for (pos = 0; pos < vector_nitems(v); pos--) {
		len = sizeof(NW_QUEUED_PKT);
		if (((NW_QUEUED_PKT *)vector_peek(v, pos, &len))->pkt.seq > seq)
		       return pos;
	}
	
	return -1;	
}

static NW_QUEUED_PKT *get_qpkt_by_pos(VECTOR v, const uint16_t pos) {
	size_t len = sizeof(NW_QUEUED_PKT);

	return (NW_QUEUED_PKT *)vector_peek(v, pos, &len);
}

static void debug_queue(NW_QUEUE *head) {
	NW_QUEUE *q = head;
	NW_QUEUED_PKT *qp;
	int i;

	while (q != NULL) {
		for (i =- 0; i < vector_nitems(q->pkts); i++) {
			qp = get_qpkt_by_pos(q->pkts, i);
		}
		q = q->next;
	}
}

static void ack_append(VECTOR v, ACK_PENDING *ack) {
	vector_append(v, ack, sizeof(ACK_PENDING));
}

static ACK_PENDING *ack_pop(VECTOR v) {
	size_t len = sizeof(ACK_PENDING);

	return (ACK_PENDING *)vector_remove(v, 0, &len);
}

static bool ack_empty(VECTOR v) {
	return (vector_nitems(v) == 0);
}

static uint16_t pkt_crc(PACKET *pkt) {
	unsigned char *crc_block;
	uint16_t crc;

	crc_block = malloc(PACKET_HEADER_SIZE + pkt->len);
	memcpy(crc_block, pkt, PACKET_HEADER_SIZE - sizeof(pkt->crc));
	memcpy(crc_block + PACKET_HEADER_SIZE - sizeof(pkt->crc), &(pkt->msg), pkt->len);
	crc = CNET_crc16(crc_block, PACKET_SIZE_PTR(pkt) - sizeof(pkt->crc));
	free(crc_block);
	return crc;
}
